#include "atomic_0701_hjrd.h"

// 初始化风场模型
void initWindModel(WindModel *model, double speed, double direction, double turbulence)
{
    model->speed = speed;
    model->direction = direction;
    model->altitude_factor = 1.0;
    model->turbulence = turbulence;
}

// 更新风场模型
void updateWindModel(WindModel *model, double altitude, double timestamp)
{
    // 风速随高度变化 (边界层效应)
    model->altitude_factor = log(altitude / 0.1) / log(10000.0 / 0.1);
    model->altitude_factor = fmax(fmin(model->altitude_factor, 1.5), 0.5);

    // 湍流模拟
    model->turbulence = model->turbulence * 0.95 + 0.05 * (sin(timestamp * 2.0) + sin(timestamp * 3.7) + sin(timestamp * 5.3)) / 3.0;
}

// 环境扰动补偿函数
int compensateEnvironmentalDisturbance(EnvironmentalCompensationInput *input,
                                       EnvironmentalCompensationOutput *output)
{
    static WindModel wind_model;
    static int is_initialized = 0;

    if (!is_initialized)
    {
        initWindModel(&wind_model, input->wind_speed, input->wind_direction,
                      input->turbulence_intensity);
        is_initialized = 1;
    }

    // 更新风场模型
    updateWindModel(&wind_model, input->missile_altitude, input->timestamp);

    // 1. 计算风效应
    double wind_speed = wind_model.speed * wind_model.altitude_factor;
    double wind_x = wind_speed * cos(wind_model.direction * M_PI / 180.0);
    double wind_y = wind_speed * sin(wind_model.direction * M_PI / 180.0);

    // 相对风速 (考虑导弹运动)
    double missile_vx = input->missile_velocity * cos(input->missile_angle * M_PI / 180.0);
    double missile_vy = input->missile_velocity * sin(input->missile_angle * M_PI / 180.0);

    double relative_wind_x = wind_x - missile_vx;
    double relative_wind_y = wind_y - missile_vy;

    // 风效应计算
    output->missile_velocity = relative_wind_x / input->missile_velocity;
    output->missile_altitude = relative_wind_y / input->missile_velocity;
    output->missile_angle = wind_model.turbulence * 0.1;

    // 2. 湍流效应
    output->turbulence_effect = wind_model.turbulence * 0.2;

    // 3. 温度效应
    double temperature_factor = 1.0 + (input->temperature - 15.0) / 50.0;
    output->temperature_effect = temperature_factor - 1.0;

    // 4. 湿度效应
    output->humidity_effect = input->humidity / 100.0 * 0.1;

    // 5. 空气密度效应
    double standard_density = 1.225 * exp(-input->missile_altitude / 8500.0);
    output->air_density_effect = (input->air_density - standard_density) / standard_density;

    // 6. 总扰动强度
    output->total_disturbance = sqrt(output->missile_velocity * output->missile_velocity +
                                     output->missile_altitude * output->missile_altitude +
                                     output->missile_angle * output->missile_angle +
                                     output->turbulence_effect * output->turbulence_effect);

    // 7. 补偿控制指令
    output->pitch_command = input->pitch_command - output->missile_altitude * 0.5 -
                                output->turbulence_effect * 0.3;
    output->yaw_command = input->yaw_command - output->missile_velocity * 0.5 -
                              output->turbulence_effect * 0.3;
    output->roll_command = input->roll_command - output->missile_angle * 0.7;
    output->throttle_command = input->throttle_command * temperature_factor;

    // 限制补偿后指令范围
    output->pitch_command = fmax(fmin(output->pitch_command, 1.0), -1.0);
    output->yaw_command = fmax(fmin(output->yaw_command, 1.0), -1.0);
    output->roll_command = fmax(fmin(output->roll_command, 1.0), -1.0);
    output->throttle_command = fmax(fmin(output->throttle_command, 1.0), 0.0);

    // 8. 计算补偿效率
    double original_error = fabs(input->pitch_command) + fabs(input->yaw_command) +
                            fabs(input->roll_command);
    double compensated_error = fabs(output->pitch_command - input->pitch_command) +
                               fabs(output->yaw_command - input->yaw_command) +
                               fabs(output->roll_command - input->roll_command);
    output->max_actuator_rate = 1.0 - compensated_error / (original_error + 1e-6);

    // 9. 自适应补偿
    double adaptive_factor = 1.0 + 0.3 * sin(input->timestamp * 0.5);
    output->adaptive_compensation = adaptive_factor - 1.0;

    // 10. 预测补偿
    output->predictive_compensation = 0.05 * (output->total_disturbance - 0.5);

    // 11. 非线性补偿
    double mach_number = input->missile_velocity / 340.0;
    double mach_factor = (mach_number > 1.0) ? 1.0 + 0.2 * (mach_number - 1.0) : 1.0;
    output->nonlinear_compensation = mach_factor - 1.0;

    // 添加一些复杂的计算和模拟
    for (int i = 0; i < 100; i++)
    {
        // 模拟一些复杂的空气动力学效应
        double dynamic_pressure = 0.5 * input->air_density *
                                  input->missile_velocity * input->missile_velocity;

        // 更新补偿考虑动态压力
        output->pitch_command *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);
        output->yaw_command *= 1.0 + 0.01 * (dynamic_pressure / 50000.0 - 1.0);

        // 模拟科里奥利力效应
        double coriolis_effect = 2 * 7.2921159e-5 * input->missile_velocity *
                                 sin(input->missile_angle * M_PI / 180.0);
        output->yaw_command += coriolis_effect * 180.0 / M_PI * 0.0001;

        // 模拟风切变效应
        double wind_shear = wind_model.speed * 0.001 * (input->missile_altitude / 1000.0);
        output->pitch_command += wind_shear * 0.01;
    }

    return 0;
}